package org.uc3m.pfc.marker.detection;

import java.util.ArrayList;
import java.util.List;

import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;

/**
 * 
 * @author esteban
 *
 */
public final class MarkerDetector implements IMarkerDetector {

	/** Tag for log. */
	public static final String TAG = "MarkerDetection";

	/** Constant for min threshold. */
	private static final double MIN_THRESHOLD = 128;

	/** Constant for max threshold. */
	private static final double MAX_THRESHOLD = 255;

	/** Constant for min area. */
	private static final double MIN_AREA = 150;

	/** Constant for delta for arclenght. */
	private static final double DELTA_FOR_ARC = 0.02;

	/** Constant for square rows. */
	private static final int SQUARE_ROWS = 4;

	/** Constant for canonical marker size. */
	private static final double MARKER_CANONICAL_SIZE = 50;

	/** Real marker size. */
	private static final float MARKER_SIZE = 0.034f;

	/** Constant for number of points in list. */
	private static final int NUM_POINTS = 8;

	/** Canonical size. */
	private final Size canonicalSize =  new Size(MARKER_CANONICAL_SIZE, 
				MARKER_CANONICAL_SIZE);	
	
	/** Mat variable, final for performance.*/
	private final Mat grey = new Mat();
	/** Mat variable, final for performance.*/
	private final Mat dst = new Mat();

	/** Mat variable, final for performance.*/
	private final Mat hierarchy = new Mat();
	/** Mat variable, final for performance.*/
	private final Mat canonicalMarker = new Mat();
	/** Mat variable, final for performance.*/
	private final Mat pointsIn = new Mat(4, 1, CvType.CV_32FC2);
	/** Mat variable, final for performance.*/
	private final Mat pointsRes = new Mat(4, 1, CvType.CV_32FC2);

	/** List of contours, final for performance.*/
	private final ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>();
	/** List of contours, final for performance.*/
	private final ArrayList<MatOfPoint> selectedContours = 
			new ArrayList<MatOfPoint>();
	/** List of contours, final for performance.*/
	private final ArrayList<Marker> newMarkers = new ArrayList<Marker>();

	/** MatOfPoint variable, final for performance. */
	private final MatOfPoint2f contour = new MatOfPoint2f();
	/** MatOfPoint variable, final for performance. */
	private final MatOfPoint2f approxCurve = new MatOfPoint2f();
	/** MatOfPoint variable, final for performance. */
	private final MatOfPoint convex = new MatOfPoint();

	/** List of points, final for performance. */
	private final float[] points = new float[NUM_POINTS];
	
	private static MarkerDetector instance;
	
	/**
	 * Default constructor.
	 */
	private MarkerDetector() {
	}

	public static MarkerDetector getInstance() {
		
		if (instance == null) {
			instance = new MarkerDetector();
		}
		
		return instance;
	}
	
	@Override
	public List<Marker> detectMarkers(final Mat frame) {

		process(frame);
		
		return newMarkers;
	}
	
	/**
	 * Process current frame and detected markers.
	 * 
	 * @param in frame received
	 */
	public void process(final Mat in) {

		contours.clear();
		selectedContours.clear();
		newMarkers.clear();

		Imgproc.cvtColor(in, grey, Imgproc.COLOR_RGBA2GRAY);
		
		Imgproc.threshold(grey, dst, MIN_THRESHOLD, MAX_THRESHOLD, 
				Imgproc.THRESH_BINARY_INV);
		
		Imgproc.findContours(dst, contours, hierarchy, Imgproc.RETR_LIST,
				Imgproc.CHAIN_APPROX_NONE);

		for (MatOfPoint contourOriginal : contours) {

			if (Math.abs(Imgproc.contourArea(contourOriginal)) > MIN_AREA) {

				contourOriginal.convertTo(contour, CvType.CV_32FC2);

				Imgproc.approxPolyDP(contour, approxCurve,
						Imgproc.arcLength(contour, true) * DELTA_FOR_ARC, true);

				approxCurve.convertTo(convex, CvType.CV_32SC2);

				boolean square = false;
				square = isSquare2(approxCurve, convex);
				// square = isSquare(approxCurve, convex);

				if (square) {

					Marker marker = createMarker(approxCurve, contourOriginal);

					detectMarker(marker, in);
				}
			}

		}

	}
	

	/**
	 * Detect if the approximated curve is a square.
	 * 
	 * @param papproxCurve to know number of rows
	 * @param pconvex if image is convex
	 * 
	 * @return true if curve is square, false otherwise
	 */
	private boolean isSquare2(final MatOfPoint2f papproxCurve, 
			final MatOfPoint pconvex) {

		boolean square = false;

		if (papproxCurve.rows() == SQUARE_ROWS
				&& Imgproc.isContourConvex(pconvex)) {

			square = true;
		
		}		
		return square;
	}
	
//	private boolean isSquare(final MatOfPoint2f approxCurve, 
//			final MatOfPoint convex) {
//
//		boolean square = false;
//
//		if (approxCurve.rows() == 4
//				&& Imgproc.isContourConvex(convex)) {
//			
//			double maxcosine = 0;
//			Point[] list = approxCurve.toArray();
//
//			for (int j = 2; j < 5; j++) {
//				double cosine = Math.abs(angle(list[j % 4], list[j - 2],
//						list[j - 1]));
//				maxcosine = Math.max(maxcosine, cosine);
//			}
//
//			if (maxcosine < 0.3) {
//
//				square = true;
//			}
//		}
//		return square;
//	}
	
//	private double angle(final Point pt1, final Point pt2, final Point pt0) {
//		double dx1 = pt1.x - pt0.x;
//		double dy1 = pt1.y - pt0.y;
//		double dx2 = pt2.x - pt0.x;
//		double dy2 = pt2.y - pt0.y;
//		return (dx1 * dx2 + dy1 * dy2)
//				/ Math.sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)
//						+ 1e-10);
//	}	

	/**
	 * Detect if the current marker is correct.
	 * 
	 * @param marker to detect
	 * @param in current frame
	 */
	private void detectMarker(final Marker marker, final Mat in) {

		warp(in, canonicalMarker, canonicalSize, marker.toList());

		marker.setMat(canonicalMarker);
		marker.extractCode();

		// if (marker.checkBorder()) {

		int id = marker.calculateMarkerId();
		if (id != -1 && id != 0) {

			if (!newMarkers.contains(marker)) {

				newMarkers.add(marker);
			}

		}
		// }

	}

	/**
	 * Create a new marker from the curve.
	 * 
	 * @param papproxCurve detected
	 * @param contourOriginal contour of the curve.
	 * 
	 * @return marker object.
	 */
	private Marker createMarker(final MatOfPoint2f papproxCurve,
			final MatOfPoint contourOriginal) {

		papproxCurve.get(0, 0, points);

		int i = -1;
		
		List<Point> p = new ArrayList<Point>();

		p.add(new Point(points[++i], points[++i]));
		p.add(new Point(points[++i], points[++i]));
		p.add(new Point(points[++i], points[++i]));
		p.add(new Point(points[++i], points[++i]));
		
		Marker marker = new Marker(MARKER_SIZE, p);
		marker.setContour(contourOriginal);

		return marker;
	}

	/**
	 * This fits a mat containing 4 vertices captured through the camera into a
	 * canonical mat.
	 * 
	 * @param in
	 *            the frame captured
	 * @param out
	 *            the canonical mat
	 * @param size
	 *            the size of the canonical mat we want to create
	 * @param ppoints
	 *            the coordinates of the points in the "in" mat
	 */
	private void warp(final Mat in, final Mat out, final Size size, final 
			List<Point> ppoints) {

		int i = -1;
		
		pointsIn.put(0, 0, 
				ppoints.get(++i).x, ppoints.get(i).y, 
				ppoints.get(++i).x, ppoints.get(i).y, 
				ppoints.get(++i).x, ppoints.get(i).y,
				ppoints.get(++i).x, ppoints.get(i).y);		
		
		pointsRes.put(0, 0, 0, 0, size.width - 1, 0, size.width - 1,
				size.height - 1, 0, size.height - 1);
		
		final Mat m = Imgproc.getPerspectiveTransform(pointsIn, pointsRes);
		Imgproc.warpPerspective(in, out, m, size);
	}
	
}
